#include "controller.h"
#include "zmath.h"
#include <QtMath>
#include <QDebug>

controller::controller()
{

}

/*
 * getAimPoint()
 * 计算跟踪偏差
 *
 *
 * */
// path输入路径点，crtp当前的车辆位置，heading车辆的航向角，followerror跟踪偏差
bool controller::getAimPoint(QVector<QPointF> path,QPointF crtp,double heading,double *followError)
{

    QPointF tfootp,taimp;
    double tfootdist;
    double theta;

    if(path.size()<2) return false;
    for(int i=0;i<path.size()-1;i++)
    {
        tfootp=FootPoint(path[i],path[i+1],crtp);//求出当前位置到路径上的垂足
        tfootdist=DistanceP2P(crtp,tfootp);//求这个垂足线段的长度

        if(tfootdist<la)
        {
            //垂直距离小于预瞄距离
            taimp=OffsetPoint(tfootp,path[i],path[i+1],sqrt(la*la-tfootdist*tfootdist));    // 预瞄点

            if(PointPostion(path[i],path[i+1],taimp)==0)//当预瞄点在当前路径内
            {

                if(tfootdist<0.001)
                {
                    theta=0;
                }
                else
                {
                    theta=asinf(tfootdist/la)/3.1415926*180;
                    double tAng=AngAtBaseCoordinate(tfootp,taimp,crtp);
                    //qDebug()<<"tAng"<<tAng;
                    if(tAng>0)
                    {
                        theta=-theta;
                    }
                }
                //qDebug()<<"theta"<<theta;
                //qDebug() <<  "chuizu:" << tfootp << "yumiao:" << taimp << "dangqian:" << crtp;
//                qDebug() << HeadingAng(path[i],path[i+1]);
                double tPsi=heading+HeadingAng(path[i],path[i+1]);
                //第二种修改方法
                /*
                double tPsi;
                if(abs(path[i].y()-path[i+1].y()) < 0.01)
                {
                    if(path[i+1].x() - path[i].x() > 0)
                        tPsi = heading + 90.0;
                    else
                        tPsi = heading + 270.0;
                }
                else
                    tPsi=heading+HeadingAng(path[i],path[i+1]);
                    */


                if(tPsi>180) tPsi=tPsi-360;
                if(tPsi<-180)tPsi=tPsi+360;
                *followError=tPsi+theta;

//                qDebug() << "tpsi:" << tPsi << "theta:" << theta;
//                qDebug() << "followe:" << *followError;
                return true;
            }
        }
    }
    return false;
}

char controller::PIDPositional(double *uk,double en,double Kp,double Ki,double Kd, bool Reset,double uInitV)

{
    //*uk: 输出值
    //Kp:比例系数
    //Ti:积分时间常数
    //Td:微分时间常数
    //T:采样周期，时间间隔

    if(Reset) state=1;
    switch (state)
    {
    case 0:
    {
        break;
    }
    case 1:
    {
        en_1=en;
        state=2;
        u0=uInitV;
        *uk=u0;
        sumei=en;
        break;
    }
    case 2:
    {
        sumei=sumei+en;
        *uk=Kp*en+Ki*sumei+Kd*(en-en_1)+u0;
        en_1=en;
        break;
    }
    }
    //qDebug()<<"state"<<state;//<<*uk;
    return state;
}
